/*
 * Copyright (c) 2011-2013 Jarek Sacha. All Rights Reserved.
 *
 * Author's e-mail: jpsacha at gmail.com
 */

package samples;


import br.com.manchini.stereocv.utils.MatcherUtils;
import br.com.manchini.stereocv.utils.RobustMatcher;
import com.googlecode.javacv.CanvasFrame;
import static com.googlecode.javacv.cpp.opencv_core.*;
import com.googlecode.javacv.cpp.opencv_core.IplImage;
import static com.googlecode.javacv.cpp.opencv_calib3d.*;
import com.googlecode.javacv.cpp.opencv_core;
import com.googlecode.javacv.cpp.opencv_features2d;
import static com.googlecode.javacv.cpp.opencv_highgui.*;
import static com.googlecode.javacv.cpp.opencv_features2d.*;
import com.googlecode.javacv.cpp.opencv_imgproc;
import com.googlecode.javacv.cpp.opencv_nonfree;
import java.util.ArrayList;


/** The example for section "Matching images using random sample consensus" in Chapter 9, page 233.
  *
  * Most of the computations are done by `RobustMatcher` helper class.
  */
public class Ex4MatchingUsingSampleConsensus {
    
    public DMatch toNativeVector(ArrayList<DMatch> src) {
        DMatch dest = new DMatch(src.size());
        for (int i = 0; i < src.size(); i++) {
            // Since there is no way to `put` objects into a vector DMatch,
            // We have to reassign all values individually, and hope that API will not any new ones.
            copy(src.get(i), dest.position(i));
        }
        // Set position to 0 explicitly to avoid issues from other uses of this position-based container.
        dest.position(0);

        return dest;
    }

    void copy(DMatch src, DMatch dest) {
        dest.put(src.limit(src.position() + 1));
    }

    public Ex4MatchingUsingSampleConsensus() {

        // Read input images
        IplImage image1 = cvLoadImage(("img/6esq.jpg"));
        IplImage image2 = cvLoadImage(("img/6dir.jpg"));
//    cvshow(image1, "Right image")
//    show(image2, "Left image")

        // Prepare the matcher
        RobustMatcher rMatcher = new RobustMatcher(
                0.98,
                1.0,
                0.65F,
                new opencv_nonfree.SURF(10),
                true);

        //
        // Match two images
        //


        // draw the matches
        IplImage matchesCanvas = IplImage.create(new CvSize(image1.width() + image2.width(), image1.height()), image1.depth(), 3);

//    show(matchesCanvas, "Matches")


        RobustMatcher.Result matches = rMatcher.matchImages(image1, image2);
        matches.getMatches();

        opencv_features2d.drawMatches(image1, matches.getKeyPoints1(), image2, matches.getKeyPoints2(),
                toNativeVector(matches.getMatches()), matchesCanvas, CvScalar.WHITE, cvScalarAll(-1), null, DrawMatchesFlags.DEFAULT);


        
    // Draw the epipolar lines
    CvPoint2D32f[] points = MatcherUtils.toCvPoint2D32f(matches.getMatches(), matches.getKeyPoints1(), matches.getKeyPoints2());
    CvPoint2D32f point1 = points[0];
    CvPoint2D32f point2 = points[1];
    
    CvMat lines1 = CvMat.create(point1.capacity(), 3, CV_32F, 1);
    cvComputeCorrespondEpilines(MatcherUtils.toCvMat(point1), 1, matches.getFundamentalMatrix(), lines1);
    
//    show(drawEpiLines(image2, lines1, points2), "Left Image Epilines (RANSAC)")
            
    CvMat lines2 = CvMat.create(point2.capacity(), 3, CV_32F, 1);
    
    cvComputeCorrespondEpilines(MatcherUtils.toCvMat(point2), 2, matches.getFundamentalMatrix(), lines2);
//    show(drawEpiLines(image1, lines2, points1), "Right Image Epilines (RANSAC)")
    
     CanvasFrame canvasFrame = new CanvasFrame("Esq");
     canvasFrame.showImage(image1);
    
    }
    
    
    public static void main(String[] args) {
        
        new Ex4MatchingUsingSampleConsensus();
    }
    
    

  


    //----------------------------------------------------------------------------------------------------------------


    /** Draw `epilines` and `points` on a color copy of an `image`.
      *
      * @return new image  with epilines and points.
      */
    private IplImage drawEpiLines(IplImage image, CvMat epilines, CvPo